#include <ros/ros.h>
#include <nav_msgs/OccupancyGrid.h>
#include <geometry_msgs/Polygon.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include <std_msgs/Int32.h>
#include <cstdlib>
#include <string.h>
#include <signal.h>
#include <termios.h>
#include <stdio.h>
#include <math.h>
#include <vector>
#include "roboard_drivers/Motor.h"
#include "roboard_drivers/Encoder.h"
#include "gr8/Distance.h"


using namespace gr8;
using namespace roboard_drivers;
using std::vector;

ros::Subscriber	direction_sub;	
ros::Subscriber	enc_sub;
ros::Subscriber sensor_sub;
ros::Subscriber detection_sub;	

ros::Publisher mot_pub;
ros::Publisher robot_marker_pub;
ros::Publisher arrow_marker_pub;
ros::Publisher tag_marker_array_pub;
ros::Publisher map_pub;
ros::Publisher scaled_map_pub;

/* -------------------------- CONTROLLER VARIABLES -------------------------- */


int   keyboard_control	= 0;	// [0, 1] UP and STOP function whether this is true or not
float run_robot 	= 0;		// [0, 1] turns motors on or off
int   TURNING		= 0;	// [0, 1] flag indicating if robot is currently turning 90 deg
int   FOUND_RIGHT	= 0;	// [0, 1] flag indicating if right wall is found (similar to right_wall_detected but need a separate flag)

// Variables for PID controller
float Kp_dist  = 3.0*1.2;   	 float Ki_dist  = 0.2*1.2;		float Kd_dist  = 0.2*1.2;
float Kp_theta = 1.5;  		 float Ki_theta = 0.2;		float Kd_theta = 0.1;
float scale = 0.7;
float motor_brake = 0.18;

// Variable for time
struct timeval t;
double timestamp_current = 0;
double timestamp_previous = 0;
double dt = 0;

// Constant variables
const float PI = M_PI;
float r = 0.037;         		// [cm] wheel radius
float L = 0.224/2.0;     		// [cm] wheel dist to center of rotation (axle length/2)
const float Enc = 226.75;	// [ticks/rev] number of ticks in one rev
const float K  =  Enc/(2.0*PI*r);  // [ticks/cm] conversion factor

// Other variables for controller

float dist_ref = 0;		// [cm] within [0, 20]
float theta_ref = 0;		// [deg] within [0, 180]
float theta_correction = -3;	// [deg]
int theta_from_IR = 0;		// [0, 1] flag indicating if theta is coming from IR's or encoders

float current_dist_ticks = 0;
float current_dist_ticks_previous = 0;
float current_dist_cm = 0;
float current_wheeldiff_ticks = 0;
float current_theta_deg = 0;

float e_p_dist = 0;  float e_p_dist_previous = 0;  float e_i_dist = 0;  float e_d_dist = 0;  // [0, 1] corresponds to [0, 20]
float e_p_theta = 0; float e_p_theta_previous = 0; float e_i_theta = 0; float e_d_theta = 0; // [0, 1] corresponds to [0, 180]
float u_p_dist = 0; float u_i_dist = 0; float u_d_dist = 0; float u_p_theta = 0; float u_i_theta = 0; float u_d_theta = 0;
float u_dist = 0;	    // [0, 1]
float u_theta = 0;	    // [0, 1]
float u_total_R = 0;
float u_total_L = 0;

// Variables for the state machine
int robot_state = 0;
#define STOP				0  //
#define TOO_CLOSE_RIGHT			1  //
#define TOO_CLOSE_LEFT			2  //
#define TOO_FAR_RIGHT			3  //
#define TOO_FAR_LEFT			4  //
#define ALIGN_RIGHT_NOSE_TOO_FAR	5  //
#define ALIGN_RIGHT_NOSE_TOO_CLOSE	6  //
#define ALIGN_LEFT_NOSE_TOO_FAR		7  //
#define ALIGN_LEFT_NOSE_TOO_CLOSE	8  //
#define TURN_LEFT			9  //
#define TURN_RIGHT			10 //
#define GO_FORWARD			11 //
#define PLANNING_PATH			12 //
#define SETTING_GOAL			13 //
#define SEEKING_GOAL			14 //
#define TURNINGSTATE			15 //
#define WAIT				16 //
#define ALIGN_RIGHT			17 //


/* -------------------------- PATH PLANNING VARIABLES -------------------------- */

bool left_wall_detected_MAP  = false;
bool right_wall_detected_MAP = false;
bool front_wall_close_MAP = false;
bool left_wall_detected_IR  = false;
bool right_wall_detected_IR = false;
bool front_wall_close_IR = false;

bool inside_corner = false;

bool local_unknowns = true;		// true if there are unknowns inside map_local
bool path_found = false;			// true if robot has found a path to the goal
bool arrived = false;			// true if robot has arrived at the goal

int x_cell = 0;			  	// [cell] x coordinate of robot's current cell
int y_cell = 0;			  	// [cell] y coordinate of robot's current cell

int FRONT_THRESHOLD = 6;			// [cells] number of cells to front_wall_close
int range = 13;	  			// [cells] defines size of map_local
int UNKNOWN = 50;			// the character in the map that represents an unknown
int SPACE = 0;				// the character in the map that represents a open space
int WALL = 100;
vector<int> unknown_x;			// [cell] array holding the x values of unknowns
vector<int> unknown_y;			// [cell] array holding the y values of unknowns
float dist_to_goal = 0;			// [cells] euclidian straight line distance from robot to goal
float dist_to_goal_shortest = 99999;  	// [cells] spot to save the current shortest distance to goal
int dist_to_goal_path = 0;		// [cells] distance to the goal along the path
int goal_x = 0;			// [cell]
int goal_y = 0;			// [cell]
int wave_step = 0;		// the current wave step
int path_step = 0;		// the current path step
vector<int> nextcells_x;		// array holding the x values of the next cells to expand the wave front to
vector<int> nextcells_y;		// array holding the y values of the next cells to expand the wave front to

#define NORTH_ref 90
#define SOUTH_ref 270
#define EAST_ref 0
#define WEST_ref 180
#define north	0
#define south	1
#define east	2
#define west	3

/* -------------------------- MAPPING VARIABLES -------------------------- */
#define HEIGHT 1000
#define WIDTH 1000
#define RESOLUTION 2
#define OFFSET 500

float dist_F = 0;	// [cm]
float dist_R = 0;	// [cm]
float dist_L = 0; 	// [cm]

float dist_right_map = 0;
float dist_left_map = 0;
float dist_front_map = 0;

int heading = 0;	// 0, 1, 2, 3 = north, south, east, west

float dist_F1 = 0;	// [cm]
float dist_F2 = 0;	// [cm]
float dist_RR = 0;	// [cm] right rear
float dist_RF = 0; 	// [cm] right front
float dist_LR = 0;	// [cm] left rear
float dist_LF = 0; 	// [cm] left front
float dist_sonar = 0;

float current_X = 0;	
float current_Y = 0;	
float current_theta = 0;
float theta_change = 0;

long ticks_L = 0;
long ticks_R = 0;
long prev_ticks_L = 0;
long prev_ticks_R = 0;
long initial_encoder_offset_L = 0;
long initial_encoder_offset_R = 0;
bool first_encoder_read = true;

int tag_count = 0;
int camera;
float camera_ang = 0;

float theta_R;
float theta_L;

visualization_msgs::Marker robot_marker;
visualization_msgs::Marker arrow_marker;
visualization_msgs::Marker tag_marker;
visualization_msgs::MarkerArray tag_marker_array;

nav_msgs::OccupancyGrid map;
nav_msgs::OccupancyGrid scaled_map;

/* --------------------------------------------------------------------------------------------------------- */
/* ---------------------------------------------- CONTROLLER ----------------------------------------------- */
/* --------------------------------------------------------------------------------------------------------- */

// State machine
void get_refs_from_state() {

	if (TURNING == 1) {
		if (abs(theta_ref - current_theta_deg) < 5) {TURNING = 0;}  // if turn is complete
	} else if (TURNING == 0) {
		switch (robot_state) {
			case STOP:
			  dist_ref = current_dist_cm;
			  theta_ref = current_theta_deg;
			  e_p_dist = 0;
			  e_p_dist_previous = 0;
			  e_i_dist = 0;
			  e_d_dist = 0;
			  e_p_theta = 0;
			  e_p_theta_previous = 0;
			  e_i_theta = 0;
			  e_d_theta = 0;
			  break;
			case WAIT:
			  break;
			case TURN_LEFT:
			  dist_ref = current_dist_cm;
			  switch (heading) {
				case north:
					theta_ref = NORTH_ref + 90;
					break;
				case south:
					theta_ref = SOUTH_ref + 90;
					break;
				case east:
					theta_ref = EAST_ref + 90;
					break;
				case west:
					theta_ref = WEST_ref + 90;
					break;
			  }
			  TURNING = 1;
			  break;
			case TURN_RIGHT:
			  dist_ref = current_dist_cm;
			  switch (heading) {
				case north:
					theta_ref = NORTH_ref - 90;
					break;
				case south:
					theta_ref = SOUTH_ref - 90;
					break;
				case east:
					theta_ref = EAST_ref - 90;
					break;
				case west:
					theta_ref = WEST_ref - 90;
					break;
			  }
			  TURNING = 1;
			  break;
			case GO_FORWARD:
			  e_p_dist = 0;
			  e_p_dist_previous = 0;
			  e_i_dist = 0;
			  e_d_dist = 0;
			  e_p_theta = 0;
			  e_p_theta_previous = 0;
			  e_i_theta = 0;
			  e_d_theta = 0;
			  dist_ref = current_dist_cm + 2;
			  break;
			case ALIGN_RIGHT:
			  dist_ref = current_dist_cm + 2;
			  switch (heading) {
				case north:
					theta_ref = NORTH_ref;
					break;
				case south:
					theta_ref = SOUTH_ref;
					break;
				case east:
					theta_ref = EAST_ref;
					break;
				case west:
					theta_ref = WEST_ref;
					break;
			  }
			  break;
			case TOO_CLOSE_RIGHT:
			  dist_ref = current_dist_cm + 1;
			  switch (heading) {
				case north:
					theta_ref = NORTH_ref + 8;
					break;
				case south:
					theta_ref = SOUTH_ref + 8;
					break;
				case east:
					theta_ref = EAST_ref + 8;
					break;
				case west:
					theta_ref = WEST_ref + 8;
					break;
			  }
			  break;
			case TOO_FAR_RIGHT:
			  dist_ref = current_dist_cm + 1;
			  switch (heading) {
				case north:
					theta_ref = NORTH_ref - 8;
					break;
				case south:
					theta_ref = SOUTH_ref - 8;
					break;
				case east:
					theta_ref = EAST_ref - 8;
					break;
				case west:
					theta_ref = WEST_ref - 8;
					break;
			  }
			  break;
		}
	}
}


// Distance PID controller
void calc_distance_control() {
	
	current_dist_ticks = (ticks_R  + ticks_L)/2.0;
	current_dist_cm = current_dist_ticks/K;
	e_p_dist = (dist_ref*K - current_dist_ticks)/(K*20.0);
	e_i_dist += e_p_dist*dt;
	e_d_dist = (e_p_dist - e_p_dist_previous)/dt;
	e_p_dist_previous = e_p_dist;
	u_p_dist = Kp_dist*e_p_dist;
	u_i_dist = Ki_dist*e_i_dist;
	u_d_dist = Kd_dist*e_d_dist;
	u_dist =  u_p_dist + u_i_dist + u_d_dist;
}

// Theta PID controller
void calc_theta_control() {

	e_p_theta = (theta_ref - current_theta_deg)/180.0;
	e_i_theta += e_p_theta*dt;
	e_d_theta = (e_p_theta - e_p_theta_previous)/dt;
	e_p_theta_previous = e_p_theta;
	u_p_theta = Kp_theta*e_p_theta;
	u_i_theta = Ki_theta*e_i_theta;
	u_d_theta = Kd_theta*e_d_theta;
	u_theta =  u_p_theta + u_i_theta  + u_d_theta;
}



/* ------------------------------------------------------------------------------------------------------------ */
/* ---------------------------------------------- PATH PLANNING ----------------------------------------------- */
/* ------------------------------------------------------------------------------------------------------------ */

// This function checks a local area around the robot for unknowns and walls
void check_local_area() {
	local_unknowns = 0;
	right_wall_detected_MAP = false;
	left_wall_detected_MAP = false;
	front_wall_close_MAP = false;

	for(int k = -range; k < range; k++) {			// search local map
		for (int kk = -range; kk < range; kk++) {
			if (scaled_map.data[(y_cell+k)*WIDTH+x_cell+kk] == UNKNOWN) {local_unknowns = 1;} // for unknown character
			
			switch (heading) {

				case north:
					if (scaled_map.data[(y_cell+k)*WIDTH+x_cell+kk] == WALL) {	 // for left or right walls and front walls
						if (kk >= 0 && k == 0) {
							right_wall_detected_MAP = true;
							dist_right_map = abs(kk*RESOLUTION);
						}
						else if (k == 0 && !right_wall_detected_MAP) {
							left_wall_detected_MAP = true;
							dist_left_map = abs(kk*RESOLUTION);
						} 
						if (k < FRONT_THRESHOLD && k > 0 && abs(kk) == 0) {
							front_wall_close_MAP = true;
							dist_front_map = abs(k*RESOLUTION);
						} 
					}
					break;
				case south:
					if (scaled_map.data[(y_cell+k)*WIDTH+x_cell+kk] == WALL) {
						if (kk <= 0 && k == 0) {
							right_wall_detected_MAP = true;
							dist_right_map = abs(kk*RESOLUTION);
						}
						else if (k == 0 && !right_wall_detected_MAP) {
							left_wall_detected_MAP = true;
							dist_left_map = abs(kk*RESOLUTION);
						}
						if (k < 0 && k > -FRONT_THRESHOLD && abs(kk) == 0) {
							front_wall_close_MAP = true;
							dist_front_map = abs(k*RESOLUTION);
						}
					}
					break;
				case east:
					if (scaled_map.data[(y_cell+k)*WIDTH+x_cell+kk] == WALL) {
						if (k <= 0 && kk == 0) {
							right_wall_detected_MAP = true;
							dist_right_map = abs(k*RESOLUTION);
						}
						else if (kk == 0 && !right_wall_detected_MAP) {
							left_wall_detected_MAP = true;
							dist_left_map = abs(k*RESOLUTION);
						}
						if (kk < FRONT_THRESHOLD && kk > 0 && abs(k) == 0) {
							front_wall_close_MAP = true;
							dist_front_map = abs(kk*RESOLUTION);
						}
					}
					break;
				case west:
					if (scaled_map.data[(y_cell+k)*WIDTH+x_cell+kk] == WALL) {
						if (k >= 0 && kk == 0) {
							right_wall_detected_MAP = true;
							dist_right_map = abs(k*RESOLUTION);
						}
						else if (kk == 0 && !right_wall_detected_MAP) {
							left_wall_detected_MAP = true;
							dist_left_map = abs(k*RESOLUTION);
						}
						if (kk < 0 && kk > -FRONT_THRESHOLD && abs(k) == 0) {
							front_wall_close_MAP = true;
							dist_front_map = abs(kk*RESOLUTION);
						}
					}
					break;
			}
		}
	}
}

// This function sets the appropriate state of the robot for wall following mode
int follow_walls() {
	right_wall_detected_IR = false;
	left_wall_detected_IR = false;
	front_wall_close_IR = false;

	if (dist_RF < range*RESOLUTION || dist_RR < range*RESOLUTION)   {right_wall_detected_IR = true;}		// check walls
	else				    				{right_wall_detected_IR = false;}

	if (dist_LF < range*RESOLUTION || dist_LR < range*RESOLUTION) 	{left_wall_detected_IR  = true;}
	else 	                         				{left_wall_detected_IR  = false;}

	if (dist_F1 < FRONT_THRESHOLD*RESOLUTION && dist_F2 < FRONT_THRESHOLD*RESOLUTION)   	{front_wall_close_IR = true;}
	else 	                         							{front_wall_close_IR = false;}

	float diff_R = abs(dist_RF - dist_RR);
	float diff_L = abs(dist_LF - dist_LR);

	inside_corner = false;
	if (TURNING == 0  && (dist_RF > 12 && dist_RR < 8)) {inside_corner = true;}

	if ((front_wall_close_IR || (dist_sonar < 9 && dist_sonar != -1)) && robot_state == GO_FORWARD) {robot_state = STOP; return 0;}

	if (run_robot == 1 && keyboard_control == 0 && TURNING == 0) {
		if (front_wall_close_MAP) {				// if front wall close
			if (right_wall_detected_MAP) {robot_state = TURN_LEFT;}
			else {robot_state = TURN_RIGHT;}
		} else if (right_wall_detected_MAP && dist_RR < range*RESOLUTION) {	// else if right wall found
			FOUND_RIGHT = 1;
			if (diff_R > 0.75) {
				robot_state = ALIGN_RIGHT;
			} else	if (dist_RF < 4 && dist_RR < 4 && (e_p_theta < 15)) {robot_state = TOO_CLOSE_RIGHT;}
			else if (dist_RF > 7 && dist_RR > 7 && (e_p_theta < 15)) {robot_state = TOO_FAR_RIGHT;}
			else {robot_state = GO_FORWARD;}
		} else if (!right_wall_detected_MAP && !right_wall_detected_IR && FOUND_RIGHT == 1) {FOUND_RIGHT = 0; robot_state = TURN_RIGHT;}  // else if right wall just ended
		else {robot_state = GO_FORWARD;}							// else no right wall

		return 1;
	}
}


/* ------------------------------------------------------------------------------------------------------ */
/* ---------------------------------------------- MAPPING ----------------------------------------------- */
/* ------------------------------------------------------------------------------------------------------ */

bool wallTest(int i, int j) {
	int wallCountHorizontal = 0;
	int wallCountVertical = 0;

	for(int k=-4; k < 5; k++){
		for(int l=-1; l < 2; l++){
			if(map.data[(i+OFFSET+k)*map.info.width+(j+OFFSET+l)] == 100) {wallCountVertical++;}
			if(map.data[(i+OFFSET+l)*map.info.width+(j+OFFSET+k)] == 100) {wallCountHorizontal++;}
		}
	}

	if (wallCountVertical > 2 || wallCountHorizontal > 2) {return true;}
	else {return false;}
}

// Scaling of the map
void map_scaling() {

	for(int i = -2*range-5; i < 2*range-5; i++)	{		// updating local area around robot in scaled map based on normal map
                for(int j = -2*range-5; j < 2*range-5; j++) {
			if (map.data[(y_cell + i)*WIDTH + j + x_cell] == 0) {scaled_map.data[(y_cell + i)*WIDTH + j + x_cell] = 0;}
			else {scaled_map.data[(y_cell + i)*WIDTH + j + x_cell] = 50;}
		}
	}

	for(int i = -2*range; i < 2*range; i++) {	// scales wall cells in scaled_map k x l cells bigger if wallTest passes
		for(int j = -2*range; j < 2*range; j++) {
		        if ((map.data[(y_cell + i)*WIDTH + j + x_cell] == 100) && (wallTest(i + y_cell - OFFSET, j + x_cell - OFFSET))) {
				for(int k = -4; k < 5; k++) {
					for (int l = -4; l < 5; l++) {
						scaled_map.data[(i + y_cell + l)*WIDTH + j + k + x_cell] = 100;
					}
				}
			}
		}
	}

	if (inside_corner) {
		if (heading == north || heading == south) {
			for(int i = -2*range; i < 2*range; i++) {	// scales inside corner walls extra
				for(int j = -2*range; j < 2*range; j++) {
					if ((map.data[(y_cell + i)*WIDTH + j + x_cell] == 100) && (wallTest(i + y_cell - OFFSET, j + x_cell - OFFSET))){
						for(int k = -4; k < 5; k++) {
							for (int l = -6; l < 7; l++) {
								scaled_map.data[(i + y_cell + l)*WIDTH + j + k + x_cell] = 100;
							}
						}
					}
				}
			}
		}

		if (heading == east || heading == west) {
			for(int i = -2*range; i < 2*range; i++) {	 
				for(int j = -2*range; j < 2*range; j++) {
					if ((map.data[(y_cell + i)*WIDTH + j + x_cell] == 100) && (wallTest(i + y_cell - OFFSET, j + x_cell - OFFSET))){
						for(int k = -6; k < 7; k++) {
							for (int l = -4; l < 5; l++) {
								scaled_map.data[(i + y_cell + l)*WIDTH + j + k + x_cell] = 100;
							}
						}
					}
				}
			}
		}
	}
}

void localization() {
	
	theta_R = atan((dist_RF - dist_RR)/15.5)*180/PI;
	theta_L = atan((dist_LF - dist_LR)/15.5)*180/PI;

	if ((theta_change < 0.5) && (abs(theta_R) < 5) && (abs(theta_L) < 5)) {
		if (((dist_F - dist_front_map) > 0) && (dist_front_map != 0) && (dist_F < range) && ((dist_F - dist_front_map) < 15)) {
			current_X = current_X - cos(current_theta*PI/180)*(dist_F - dist_front_map)/100;
			current_Y = current_Y - sin(current_theta*PI/180)*(dist_F - dist_front_map)/100;
		}	
	}
}


void raytrace(float sensor_theta, float sensor_dist, float sensor_ang, float sensor_reading, bool wall){
	
	float dist_sum = 0;
	float ray_x;
	float ray_y;
	ray_x = current_X*100 + cos(sensor_ang*PI/180 + current_theta*PI/180)*sensor_dist;
	ray_y = current_Y*100 + sin(sensor_ang*PI/180 + current_theta*PI/180)*sensor_dist;

	float dist = 1;
	bool update_pos = false;
	int i = 0;
	int j = 0;
	int last_i = -1;
	int last_j = -1;

	while (dist_sum <= sensor_reading && update_pos == false) {
		ray_x = ray_x + cos(sensor_theta*PI/180)*dist;
		ray_y = ray_y + sin(sensor_theta*PI/180)*dist;
		dist_sum = dist_sum + dist;
		j = static_cast<int>((ray_x + 0.5)/2);
		i = static_cast<int>((ray_y + 0.5)/2);
		if((i < HEIGHT) && (j < WIDTH)){
			if(!(i == last_i && j == last_j)){
				if(map.data[(i+OFFSET)*map.info.width+(j+OFFSET)] > 0) {map.data[(i+OFFSET)*map.info.width+(j+OFFSET)] += -50;}
				last_i = i;
				last_j = j;
			}
		}
		else printf("Pos outside of map size: %d : %d \n", i, j);

	}
	if((i < HEIGHT) && (j < WIDTH) && update_pos == false) {
		if(wall && map.data[(i+OFFSET)*map.info.width+(j+OFFSET)] < 100) {map.data[(i+OFFSET)*map.info.width+(j+OFFSET)] = 100;}
	}	

}


/* ------------------------------------------------------------------------------------------------------------------ */
/* ---------------------------------------------- INTERRUPT FUNCTIONS ----------------------------------------------- */
/* ------------------------------------------------------------------------------------------------------------------ */

// Recieves a int if the robot has detected a tag and then publishes a blue sphere figure to the map
void tagDetection(const std_msgs::Int32::ConstPtr& msg)
{
	if(int(msg->data) == 0) 
	{
		camera = 0;
		camera_ang = current_theta + 90;
	} else
	{
		camera = 1;
		camera_ang = current_theta - 90;
	}

	printf("Tag detected from camera: %d\n", camera);

	uint32_t sphere = visualization_msgs::Marker::SPHERE;

	// Set the frame ID and timestamp.
	tag_marker.header.frame_id = "/map";
	tag_marker.header.stamp = ros::Time::now();

	// Set the namespace and id for this robot_marker.  This serves to create a unique ID
	// Any robot_marker sent with the same namespace and id will overwrite the old one
	tag_marker.ns = "tags";
	tag_marker.id = tag_count;

	// Set the robot_marker type.
	tag_marker.type = sphere;

	// Set the robot_marker action.  Options are ADD and DELETE
	tag_marker.action = visualization_msgs::Marker::ADD;

	// Set the scale of the robot_marker -- 1x1x1 here means 1m on a side
	tag_marker.scale.x = 4.0;
	tag_marker.scale.y = 4.0;
	tag_marker.scale.z = 1.0;

	// Set the color -- be sure to set alpha to something non-zero!
	tag_marker.color.r = 1.0f;
	tag_marker.color.g = 0.0f;
	tag_marker.color.b = 0.0f;
	tag_marker.color.a = 1.0;

	tag_marker.pose.position.x = current_X*100;
	tag_marker.pose.position.y = current_Y*100;

	tag_marker.lifetime = ros::Duration();

	tag_marker_array.markers.push_back(tag_marker);

	tag_marker_array_pub.publish(tag_marker_array);
	tag_count++;
}


void sensorData(const Distance::ConstPtr& msg)
{
	dist_R = (msg->val1 + msg->val3)/2;		// distances are averages of IR readings for now
	dist_L = (msg->val0 + msg->val2)/2;
	dist_F = (msg->val4 + msg->val5)/2;

	dist_RR = msg->val1;  // right rear
	dist_RF = msg->val3;  // right front
	dist_LR = msg->val0;  // left rear
	dist_LF = msg->val2;  // left front
	dist_F1 = msg->val4;  // front 1
	dist_F2 = msg->val5;  // front 2

	dist_sonar = msg->sonar;
	
	theta_R = atan((dist_RF - dist_RR)/15.5)*180/PI;
	theta_L = atan((dist_LF - dist_LR)/15.5)*180/PI;
	
	//Correct theta to the angle of the wall 
	float theta_ir = current_theta; 	

	if ((robot_state == GO_FORWARD || robot_state == ALIGN_RIGHT) && TURNING == 0 && FOUND_RIGHT == 1 && !inside_corner && dist_R < 10) 			{theta_from_IR = 1;}
	else {theta_from_IR = 0;}

	if (theta_from_IR == 1) { 
		switch (heading) {
		  case north:
			current_theta_deg = NORTH_ref + theta_R + theta_correction;
			theta_ir = fmod(NORTH_ref + theta_R, 360);
			break;
		  case south:
			current_theta_deg = SOUTH_ref + theta_R + theta_correction;
			theta_ir = fmod(SOUTH_ref + theta_R, 360);
			break;
		  case east:
			current_theta_deg = EAST_ref + theta_R + theta_correction;
			theta_ir = fmod(EAST_ref + theta_R, 360);
			break;
		  case west:
			current_theta_deg = WEST_ref + theta_R + theta_correction;
			theta_ir = fmod(WEST_ref + theta_R, 360);
			break;
		}

		if(theta_ir < 0) {theta_ir += 360;}
		current_theta = fmod(theta_ir + theta_correction, 360);

	}

	float sensor_L_theta = fmod (current_theta + 90, 360);
	float sensor_R_theta = fmod (current_theta - 90, 360);

	// dist, ang, Theta
	float sensor_R2_pos[] = { 11.66, -59.0, sensor_R_theta }; // Front-Right
	float sensor_R1_pos[] = { -14.14, 45.0, sensor_R_theta }; // Back-Right

	float sensor_L2_pos[] = { 11.66, 59.0, sensor_L_theta };  // Front-Left
	float sensor_L1_pos[] = { -14.14, -45.0,sensor_L_theta }; // Back-Left

	float sensor_F1_pos[] = { 14.14, 45.0, current_theta };    // Left
	float sensor_F2_pos[] = { 14.14, -45.0, current_theta };   // Right

	float sensor_sonar_pos[] = { 10.0, 0.0, current_theta };
 
	if(dist_RR < 25)
		raytrace(sensor_R1_pos[2], sensor_R1_pos[0], sensor_R1_pos[1], dist_RR, true);
	else
		raytrace(sensor_R1_pos[2], sensor_R1_pos[0], sensor_R1_pos[1], dist_RR, false);
	if(dist_RF < 25)
		raytrace(sensor_R2_pos[2], sensor_R2_pos[0], sensor_R2_pos[1], dist_RF, true);
	else
		raytrace(sensor_R2_pos[2], sensor_R2_pos[0], sensor_R2_pos[1], dist_RF, false);

	if(dist_LR < 25)
		raytrace(sensor_L1_pos[2], sensor_L1_pos[0], sensor_L1_pos[1], dist_LR, true);
	else
		raytrace(sensor_L1_pos[2], sensor_L1_pos[0], sensor_L1_pos[1], dist_LR, false);
	if(dist_LF < 25)
		raytrace(sensor_L2_pos[2], sensor_L2_pos[0], sensor_L2_pos[1], dist_LF, true);
	else
		raytrace(sensor_L2_pos[2], sensor_L2_pos[0], sensor_L2_pos[1], dist_LF, false);

	if(dist_F1 < 45)
		raytrace(sensor_F1_pos[2], sensor_F1_pos[0], sensor_F1_pos[1], dist_F1, true);
	else
		raytrace(sensor_F1_pos[2], sensor_F1_pos[0], sensor_F1_pos[1], dist_F1, false);
	if(dist_F2 < 45)
		raytrace(sensor_F2_pos[2], sensor_F2_pos[0], sensor_F2_pos[1], dist_F2, true);
	else
		raytrace(sensor_F2_pos[2], sensor_F2_pos[0], sensor_F2_pos[1], dist_F2, false);

	if(dist_sonar != -1 && dist_sonar < 40) {
		raytrace(sensor_sonar_pos[2],   sensor_sonar_pos[0], sensor_sonar_pos[1], dist_sonar, true);
		raytrace(sensor_sonar_pos[2]+3, sensor_sonar_pos[0], sensor_sonar_pos[1], dist_sonar, true);
		raytrace(sensor_sonar_pos[2]+6, sensor_sonar_pos[0], sensor_sonar_pos[1], dist_sonar, true);
		raytrace(sensor_sonar_pos[2]-3, sensor_sonar_pos[0], sensor_sonar_pos[1], dist_sonar, true);
		raytrace(sensor_sonar_pos[2]-6, sensor_sonar_pos[0], sensor_sonar_pos[1], dist_sonar, true);
	}
	else if(dist_sonar != -1) {
		raytrace(sensor_sonar_pos[2],   sensor_sonar_pos[0], sensor_sonar_pos[1], dist_sonar, false);
		raytrace(sensor_sonar_pos[2]+3, sensor_sonar_pos[0], sensor_sonar_pos[1], dist_sonar, false);
		raytrace(sensor_sonar_pos[2]+6, sensor_sonar_pos[0], sensor_sonar_pos[1], dist_sonar, false);
		raytrace(sensor_sonar_pos[2]-3, sensor_sonar_pos[0], sensor_sonar_pos[1], dist_sonar, false);
		raytrace(sensor_sonar_pos[2]-6, sensor_sonar_pos[0], sensor_sonar_pos[1], dist_sonar, false);
	}
}

void recive_direction(const std_msgs::Int32::ConstPtr &msg)
{
	    switch(msg->data)
	    {
	      case 4:
		if (keyboard_control == 1) {
			puts("LEFT");
			dist_ref = current_dist_cm;
			theta_ref += 90;
			run_robot = 1;
			break;
		}
	      case 3:
		if (keyboard_control == 1) {
			puts("RIGHT");
			dist_ref = current_dist_cm;
			theta_ref -= 90;
			run_robot = 1;
			break;
		}
	      case 1:
			puts("UP");
			robot_state = GO_FORWARD;
			run_robot = 1;
			break;
	      case 2:
		if (keyboard_control == 1) {
			puts("DOWN");
			robot_state = WAIT;
			run_robot = 1;
			break;
		}
	      case 5:
		puts("STOP");
		robot_state = STOP;
		run_robot = 0;
		break;
	    }
}

//Callback function for the "/encoder" topic.
void recive_encoder(const Encoder::ConstPtr &msg) {

	if(first_encoder_read) {
		initial_encoder_offset_L = -msg->left;
		initial_encoder_offset_R = msg->right;
		first_encoder_read = false;
	}	
	prev_ticks_L = ticks_L;
	prev_ticks_R = ticks_R;	
	ticks_L = -msg->left - initial_encoder_offset_L;
	ticks_R = msg->right - initial_encoder_offset_R;

	timestamp_previous = timestamp_current;
	gettimeofday(&t, NULL);
	timestamp_current = t.tv_sec+(double(t.tv_usec)/1000000.0);
	dt = timestamp_current - timestamp_previous;

	long ticks_change_L = ticks_L - prev_ticks_L;
	long ticks_change_R = ticks_R - prev_ticks_R;

	float dist = (ticks_change_L + ticks_change_R)/(2.0 * K);
	float theta;

	theta_change = fmod (((ticks_change_R - ticks_change_L)*360.0*r/(Enc*2.0*L)), 360);
	
	current_theta_deg += theta_change;
	theta = current_theta + theta_change;
	if(theta < 0) {theta += 360;}
	current_theta = fmod (theta, 360);

	current_X = current_X + cos(theta*PI/180)*dist;
	current_Y = current_Y + sin(theta*PI/180)*dist;
	


/* ---------------------------------------------- PATH PLANNING ----------------------------------------------- */

	//Set the heading		
	if (current_theta > 315 || current_theta <= 45) {heading = east;}
	else if (current_theta > 45 && current_theta <= 135) {heading = north;}
	else if (current_theta > 135 && current_theta <= 225) {heading = west;}
	else if (current_theta > 225 && current_theta <= 315) {heading = south;}

	check_local_area();
	follow_walls();

/* ---------------------------------------------- CONTROLLER ----------------------------------------------- */

	get_refs_from_state();
	calc_distance_control();
	calc_theta_control();

	u_total_R = u_dist + u_theta;
	u_total_L = u_dist - u_theta;

	float motor_brake_R = motor_brake;
	float motor_brake_L = motor_brake;

	if (u_total_R < 0) {motor_brake_R = -motor_brake_R;}
	if (u_total_L < 0) {motor_brake_L = -motor_brake_L;}

	Motor motor;
	motor.right = u_total_R*run_robot*scale + motor_brake_R;
	motor.left  = u_total_L*run_robot*scale*0.96 + motor_brake_L;

	mot_pub.publish(motor);
}

/* --------------------------------------------------------------------------------------------------- */
/* ---------------------------------------------- MAIN ----------------------------------------------- */
/* --------------------------------------------------------------------------------------------------- */

int main( int argc, char** argv )
{
/* ---------------------------------------------- INITIALIZE ----------------------------------------------- */
	
	printf("Main node started\n");
	ros::init(argc, argv, "MappingNode");	 
	ros::NodeHandle n;

	map_pub 		= n.advertise<nav_msgs::OccupancyGrid>("building_map", 1);		// Publish the map for Rviz
	scaled_map_pub		= n.advertise<nav_msgs::OccupancyGrid>("scaling_map", 1); 		// Publish the scaled map for Rviz
	robot_marker_pub 	= n.advertise<visualization_msgs::Marker>("visualization_robot_marker", 1); //Publish robot position
	arrow_marker_pub 	= n.advertise<visualization_msgs::Marker>("visualization_arrow_marker", 1); //Publish robot orientation
	mot_pub			= n.advertise<Motor>("/serial/motor_speed", 1);				// Change the motorspeed
	tag_marker_array_pub	= n.advertise<visualization_msgs::MarkerArray>("visualization_tag_marker_array", 1); //Publish tag position
	

	sensor_sub 		= n.subscribe("sensors/distance", 10, sensorData);		// from sensors
	detection_sub 		= n.subscribe("detection_pub", 1, tagDetection);		// to know when a tag is detected
  	direction_sub		= n.subscribe("/serial/direction", 1, recive_direction);	// for keyboard control
	enc_sub 		= n.subscribe("/serial/encoder", 1, recive_encoder);		// from encoders
	

	// Set our initial shape type to be a cube
	uint32_t shape = visualization_msgs::Marker::CUBE;
	uint32_t arrow = visualization_msgs::Marker::ARROW;

	map.header.frame_id = "/map";
	map.header.stamp = ros::Time::now();
	    
	//Map
	map.info.width = WIDTH;
	map.info.height = HEIGHT;
	map.info.resolution = RESOLUTION;

	map.info.origin.position.x = -1000.0;
	map.info.origin.position.y = -1000.0;
	map.info.origin.position.z = 0.0;
	map.data.resize(map.info.width*map.info.height);
	
	//Initialize the map to unknown
	for(int i = 0; i < map.info.height; i++){
		for(int j = 0; j < map.info.width; j++) {		
			map.data[i*map.info.width+j] = 50;
		}
	}
	
	scaled_map = map;

	// Set the frame ID and timestamp.  See the TF tutorials for information on these.
	robot_marker.header.frame_id = "/map";
	robot_marker.header.stamp = ros::Time::now();

	// Set the namespace and id for this robot_marker.  This serves to create a unique ID
	// Any robot_marker sent with the same namespace and id will overwrite the old one
	robot_marker.ns = "basic_shapes";
	robot_marker.id = 0;

	// Set the robot_marker type.  Initially this is CUBE, and cycles between that and SPHERE, ARROW, and CYLINDER
	robot_marker.type = shape;

	// Set the robot_marker action.  Options are ADD and DELETE
	robot_marker.action = visualization_msgs::Marker::ADD;

	// Set the pose of the robot_marker.  This is a full 6DOF pose relative to the frame/time specified in the header
	robot_marker.pose.position.x = 0;
	robot_marker.pose.position.y = 0;
	robot_marker.pose.position.z = 0;
	robot_marker.pose.orientation.x = 0.0;
	robot_marker.pose.orientation.y = 0.0;
	robot_marker.pose.orientation.z = 0.0;
	robot_marker.pose.orientation.w = 0.0;

	// Set the scale of the robot_marker -- 1x1x1 here means 1m on a side
	robot_marker.scale.x = 2.0;
	robot_marker.scale.y = 2.0;
	robot_marker.scale.z = 1.0;

	// Set the color -- be sure to set alpha to something non-zero!
	robot_marker.color.r = 0.0f;
	robot_marker.color.g = 1.0f;
	robot_marker.color.b = 0.0f;
	robot_marker.color.a = 1.0;
	robot_marker.lifetime = ros::Duration();

	// Set the frame ID and timestamp.  See the TF tutorials for information on these.
	arrow_marker.header.frame_id = "/map";
	arrow_marker.header.stamp = ros::Time::now();

	// Set the namespace and id for this arrow_marker.  This serves to create a unique ID
	// Any arrow_marker sent with the same namespace and id will overwrite the old one
	arrow_marker.ns = "basic_shapes";
	arrow_marker.id = 0;

	// Set the arrow_marker type.  Initially this is CUBE, and cycles between that and SPHERE, ARROW, and CYLINDER
	arrow_marker.type = arrow;

	// Set the arrow_marker action.  Options are ADD and DELETE
	arrow_marker.action = visualization_msgs::Marker::ADD;

	// Set the pose of the arrow_marker.  This is a full 6DOF pose relative to the frame/time specified in the header
	arrow_marker.pose.position.x = 0;
	arrow_marker.pose.position.y = 0;
	arrow_marker.pose.position.z = 0;
	arrow_marker.pose.orientation.x = 0.0;
	arrow_marker.pose.orientation.y = 0.0;
	arrow_marker.pose.orientation.z = 0.0;
	arrow_marker.pose.orientation.w = 0.0;

	// Set the scale of the arrow_marker -- 1x1x1 here means 1m on a side
	arrow_marker.scale.x = 20.0;
	arrow_marker.scale.y = 20.0;
	arrow_marker.scale.z = 20.0;

	// Set the color -- be sure to set alpha to something non-zero!
	arrow_marker.color.r = 1.0f;
	arrow_marker.color.g = 0.0f;
	arrow_marker.color.b = 0.0f;
	arrow_marker.color.a = 1.0;

	arrow_marker.lifetime = ros::Duration();
	ros::Rate loop_rate(10);

	int count = 0;
/* ---------------------------------------------- MAIN LOOP ----------------------------------------------- */
	while (ros::ok())
	{
/* ---------------------------------------------- MAPPING ----------------------------------------------- */
		//robot position in map
		x_cell = static_cast<int>((current_X*100 + 0.5)/2)+OFFSET;
		y_cell = static_cast<int>((current_Y*100 + 0.5)/2)+OFFSET;

		robot_marker.pose.position.x = current_X*100;
		robot_marker.pose.position.y = current_Y*100;
		robot_marker.pose.orientation.x = 0.0;
		robot_marker.pose.orientation.y = 0.0;
		robot_marker.pose.orientation.z = sin(current_theta*PI/180/2);
		robot_marker.pose.orientation.w = cos(current_theta*PI/180/2);

		arrow_marker.pose.position.x = current_X*100;
		arrow_marker.pose.position.y = current_Y*100;
		arrow_marker.pose.orientation.x = 0.0;
		arrow_marker.pose.orientation.y = 0.0;
		arrow_marker.pose.orientation.z = sin(current_theta*PI/180/2);
		arrow_marker.pose.orientation.w = cos(current_theta*PI/180/2);
	
		
		//Set robot occupied space as empty
		for(int t = 0; t < 360; t=t+5){
			raytrace(t, 0, 0, 9, false);
		}	
		

		map_scaling();

		// Publish map
		if(count == 15) {
			map_pub.publish(map);
			scaled_map_pub.publish(scaled_map);
			count = 0;
		}

		count++;
		// Publish the markers
		robot_marker_pub.publish(robot_marker);
		arrow_marker_pub.publish(arrow_marker);

/* ---------------------------------------------- END MAIN LOOP ----------------------------------------------- */
		ros::spinOnce();
		loop_rate.sleep();
	}
}




